#include "control/control.h"

using namespace control;

Control::Control(ros::NodeHandle &nh, ros::NodeHandle &private_nh)
: nh_(nh),
  private_nh_(private_nh)
{
    ptr_amr_state_ = std::make_shared<AMRState>();
    ptr_msg_control_ = std::make_shared<control_msgs::control_msg>();
    ptr_msg_control_state_ = std::make_shared<control_msgs::control_state_msg>();

    Init();
}

Control::~Control()
{

}

void Control::Init()
{
    LoadParams();
    InitLog();
    LINFO("================================== Control Start ==================================");

    ptr_move_control_.reset(new MoveControl(nh_, private_nh_));
    ptr_move_control_->Init(ptr_amr_state_, ptr_msg_control_);

    pub_control_ = nh_.advertise<control_msgs::control_msg>("/control", 10);
    pub_control_state_ = nh_.advertise<control_msgs::control_state_msg>("/control_state", 10);

    sub_actuator_ = nh_.subscribe("/actuator", 10, &Control::ActuatorCallback, this);
    sub_trajectory_ = nh_.subscribe("/trajectory", 10, &Control::TrajectoryCallback, this);
    sub_localization_ = nh_.subscribe("/localization", 10, &Control::LocalizationCallback, this);
}

void Control::Start()
{
    ros::Rate loop(20);
    while (ros::ok())
    {
        ptr_move_control_->Process();
        UpdateTotalFsm();
        pub_control_.publish(*ptr_msg_control_);
        pub_control_state_.publish(ptr_amr_state_->control_state);

        ResetTaskCommond();
        ros::spinOnce();
        loop.sleep();
    }
}

void Control::InitLog()
{
    common_lib::SpdLog::Instance()->InitSpdLog(file_path_ + "/log", "control"); 
    // TsdpClientLog::GetInstance().Init(); // TODO CSVlogForLater
}

void Control::LoadParams()
{
    private_nh_.getParam("map_file", map_file_);
    private_nh_.getParam("param_file", vehicle_param_file_);
    private_nh_.getParam("path", file_path_);
    LoadGlobalParams(file_path_, vehicle_param_file_);
}

void Control::ActuatorCallback(const actuator_msgs::actuator_msgConstPtr &msg)
{
    ptr_amr_state_->actuator = *msg;
}

void Control::TrajectoryCallback(const amr_trajectory_msgs::amr_trajectory_msgConstPtr &msg)
{
    ptr_amr_state_->msg_trajectory_task = *msg;
    ptr_amr_state_->target_state.is_receive_cmd = true;
    ptr_amr_state_->target_state.move_task_id = msg->points[0].task_id;
    ptr_amr_state_->target_state.target_point_id = msg->points.back().waypoint_id;
}

void Control::LocalizationCallback(const localization_msgs::localization_msgConstPtr &msg)
{
    ptr_amr_state_->amr_local_ = *msg;
}

void Control::UpdateTotalFsm()
{
    all_task_info_.clear();
    all_task_info_time_.clear();

    // TODO
    if (ptr_move_control_ != nullptr)
    {
        uint32_t control_move_id = 0;
        ros::Time task_time;
        FsmState control_move_fsm = FsmState::Other;

        ptr_move_control_->GetTaskInfo(control_move_id, control_move_fsm, task_time);        
        
        // // cout << "1 work task_id: " <<  control_work_id << " fsm: " << control_work_fsm << endl;
        all_task_info_.insert(pair<int, FsmState>(control_move_id, control_move_fsm));
        all_task_info_time_.insert(pair<int, ros::Time>(control_move_id, task_time));	
    }

    SummaryFsmStateForDispatch();
}

void Control::SummaryFsmStateForDispatch()
{
    // 找出所有模块中, 最大task_id的状态, 若并列最大, 则都放到统计队列中
    // 统计队列中, 必须都为Finish或者Fail, 整体才是idle, 否则为work
    cur_task_id_ = 0;
    cur_fsm_state_ = "default";
    ros::Time cur_task_time(0.0, 0.0);
    
    for (std::multimap<uint32_t, ros::Time>::const_reverse_iterator iter=all_task_info_time_.crbegin();iter!= all_task_info_time_.crend(); ++iter)
    {
        if (iter->second.toSec() < cur_task_time.toSec())
        {
            continue;
        }
        cur_task_id_ = iter->first;
        cur_task_time = iter->second;
    }

    FsmState fsm_state =  all_task_info_.find(cur_task_id_)->second;

    auto it  = all_task_info_.find(cur_task_id_);
    int  cnt = all_task_info_.count(cur_task_id_);

    for (int i=0;i<cnt;i++)
    {
        if (it->second != fsm_state)
        {
            if (fsm_state == FsmState::Work || fsm_state == FsmState::Pause)
            {
                continue;
            }
            else if (it->second == FsmState::Work || it->second == FsmState::Pause)
            {
                fsm_state = it->second;
            }
        }
        it++;
    }

    if (fsm_state == FsmState::Fail)
    {
        cur_fsm_state_ = "fail";
    }
    else if(fsm_state == FsmState::Pause)
    {
        cur_fsm_state_ = "pause";
    }
    else if(fsm_state == FsmState::Work)
    {
        cur_fsm_state_ = "working";
    }
    else if(fsm_state == FsmState::Finish)
    {
        cur_fsm_state_ = "finish";
    }
    ptr_amr_state_->control_state.task_id = cur_task_id_;
    ptr_amr_state_->control_state.amr_task_state = cur_fsm_state_;

    UpdateAMRState();
}

void Control::UpdateAMRState()
{
    std::string amr_state = "";//当前车体的状态
    if (cur_fsm_state_ == "finish" ||
        cur_fsm_state_ == "fail")
    {
        amr_state = "idle";
    }
    if(ptr_amr_state_->actuator.battery_state.is_charge)
    {
        amr_state = "charging";
    }
    if(cur_fsm_state_ == "working")
    {
        amr_state = "working";
    }
    if(cur_fsm_state_ == "pause")
    {
        amr_state = "pause";
    }
    if (ptr_amr_state_->event == Event_e::emc_stop)
    {
        amr_state = "breakdown";
    }

    ptr_amr_state_->control_state.amr_fsm_state = amr_state;
    if ( (fabs(ptr_amr_state_->actuator.walking_speed) > FLT_EPSILON) && (fabs(ptr_amr_state_->actuator.angle_speed) > FLT_EPSILON))
        ptr_amr_state_->control_state.move_state = "moving";
    else
        ptr_amr_state_->control_state.move_state = "still";
}

void Control::PostProcess()
{

}

// TODO For Cancel Task
void Control::ResetTaskCommond()
{

}